State blocks

A state block is one of the partitions of the state vector of the problem.

As a partition of the full state, each state block has its own entity. Examples of state blocks are:

  • A position of a point in 2D or 3D

  • An orientation in space (an angle in the plane, a quaternion, etc)

  • A velocity vector

  • The calibration parameters of a particular sensor

  • The bias of the measurements of a sensor

A state block has its own abstract class in WOLF. Check the StateBlock class code on GitLab Derived state block classes in WOLF are:

  • StatePoint2d and StatePoint3d for positions in 2D and 3D.

  • StateAngle for 2D orientations (angles).

  • StateQuaternion for 3D orientations (quaternions).

  • StateVector2d and StateVector3d for vectors in 2D and 3D (such as velocity).

  • StateHomogeneous3d for homogeneous coordinates in 3D.

  • StateParam<N> for parameter state blocks of size N.

The StateBlock class

The StateBlock class is the base class for all state blocks in WOLF. It has the following main attributes:

  • type A string indicating the type of the state block, such as “StatePoint2d”, “StateAngle”, etc.

  • state_ A vector (usually small) with the state values.

  • fixed_ A boolean indicating if the state block is fixed or not. See below.

  • local_param_ptr_ A pointer to a local parametrization of the error of this state block. See below.

A handy property of a state block is its fixedness that can be modified with the methods fix() and unfix(),

  • A fixed state block acts merely as a set of fixed parameters. For example, the intrinsic parameters of a camera can be defined with a state block k = [u0 v0 au av] which is declared fixed.

  • A non-fixed state block acts as a vector of variables that wants to be estimated. For example, the position of the robot at a given moment must be estimated by the solver. Sensor parameters can also be declared non-fixed, in which case they will be estimated. This allows us to easily implement systems with parameters that can be self-calibrated.

WOLF treats fixed state blocks as parameters, exactly as if they had been defined as constants, and only considers non-fixed blocks as variables to be estimated by the solver.

Note

In WOLF, state blocks can be fixed or unfixed in runtime and the solver automatically detects this change.

Local parametrization

A state block can have a local parametrization, which is a parameterization of the error of the state block. This is useful for state blocks that have a redundant representation, such as quaternions.

The local parametrization defines how to compute the error of the state block and how to update the state block with the error. This will be used by the solver to compute the updates in minimal representation and update the state block accordingly.

The abstract class LocalParametrizationBase defines the interface for local parametrizations. The derived classes have to implement the following pure virtual methods:

virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
                  Eigen::Map<const Eigen::VectorXd>& _delta,
                  Eigen::Map<Eigen::VectorXd>&       _x_plus_delta) const        = 0;
virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
                             Eigen::Map<Eigen::MatrixRowXd>&    _jacobian) const = 0;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
                   Eigen::Map<const Eigen::VectorXd>& _x2,
                   Eigen::Map<Eigen::VectorXd>&       _x2_minus_x1)              = 0;
virtual bool isValid(const Eigen::VectorXd& state, double tolerance)             = 0;

In the core plugin, there are some local parametrizations already implemented:

  • LocalParametrizationAngle for angles. This is only used for guaranteeing that the angle is always in the range [-pi, pi].

  • LocalParametrizationQuaternion for quaternions.

  • LocalParametrizationHomogeneous for homogeneous coordinates in 3D.